/**
 * @file Pan_Tilt.h
 * @author Glorill 1615688664@qq.com
 * @brief 云台计算与实例
 * @version 0.1
 * @date 2022-02-04
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#ifndef PAN_TILT_H
#define PAN_TILT_H

/* Includes ------------------------------------------------------------------*/

#include "Serial_Plot.h"
#include "Pan_Tilt_AHRS.h"

#include "math.h"
#include "SRML.h"

/* Exported macros -----------------------------------------------------------*/

/* Exported types ------------------------------------------------------------*/

/* Exported variables --------------------------------------------------------*/

//TODO:待测
const float PITCH_MAX = 245;
const float PITCH_MIN = 185;
const float PITCH_BLANCE_VOLTAGE = 0;

// //灯条机械限位
// const float PITCH_LIGHT = 260; 

//水平时的云台角
const float Pitch_Initial = 210;

//朝前方时的云台角
extern float Yaw_Initial;

extern Motor_GM6020 YawMotor;
extern Motor_GM6020 PitchMotor;
extern Serial_Plot PitchData;

class C_Pan_Tilt
{
	private:
        void Pithch_Angle_Limit(void);
        void Send_Motor_Message(void);

    public:
        enum E_PanTiltState
        {
            //输出0
            DISABLE = 0,    
            //正常输出
            NORMAL = 1,     
            //pitch输出平衡力矩，yaw输出0
            BLANCE = 2,     
        } state;

        void Init(void);
        void Control(void);

        void SetPitch(float pitch_angle);
        void SetYaw(float yaw_angle);
        void SetState(E_PanTiltState _state) 
        {
             state = _state;
        };

        float GetNowPitch(void);
        float GetNowYaw(void);
        float GetTargetPitch(void);
        float GetTargetYaw(void);
};

extern C_Pan_Tilt Pan_Tilt;
extern C_PAN_TILT_AHRS pan_tilt_AHRS;

/* Exported function declarations --------------------------------------------*/

#endif

/************************ COPYRIGHT(C) USTC-ROBOWALKER **************************/
